#ifndef SLAM_ROS_SLAM_ROS_MAP_SERVER_2D_H
#define SLAM_ROS_SLAM_ROS_MAP_SERVER_2D_H

#include <cstdio>
#include <cstdlib>
#include <fstream>
#include <memory>
#include <boost/filesystem.hpp>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp/logging.hpp"
#include "nav2_map_server/map_io.hpp"
#include "nav2_map_server/map_server.hpp" 
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "nav_msgs/srv/get_map.hpp"
#include "yaml-cpp/yaml.h"

namespace slam_ros {

class MapServer2D : public rclcpp::Node {
 public:
  explicit MapServer2D(
      const std::string& map_frame_id,
      const rclcpp::NodeOptions& options = rclcpp::NodeOptions());

  MapServer2D() = delete;

  bool loadMapFromYaml(
      nav_msgs::srv::GetMap::Response* map_resp_ptr,
      const std::string& path_to_yaml);

  bool loadMapFromValues(
      nav_msgs::srv::GetMap::Response* map_resp_ptr,
      std::string map_file_name,  
      double resolution,
      double origin[3], 
      int negate = 0, 
      double occ_th = 0.65, 
      double free_th = 0.196);
  
 private:
  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_pub_;
  rclcpp::Publisher<nav_msgs::msg::MapMetaData>::SharedPtr metadata_pub_;
  std::string frame_id_;
  nav_msgs::msg::MapMetaData meta_data_message_;
  nav_msgs::srv::GetMap::Response map_resp_;
};

}  // namespace slam_ros

#endif